#include "dmakemotecdrive.h"

DmakeMotecDrive::DmakeMotecDrive( QObject* parent ) {
    time = 50;

#ifdef NJLAKCAN
    canDevice = new NJLikeCanDevice();
#endif
#ifdef ZLGCAN
    canDevice = new ZLGCanDevice();
#endif

    Zero_Pulse                       = -2407666;
    pulse_per                        = 131072;  //电机一圈脉冲数值
    Veer_Wheel_Speed_Reduction_Radio = 64.0;
    pos_mode_speed                   = 1000;  //舵轮转角 rmp
    pos_mode_acc                     = 500;   // rps^2
    pos_mode_dec                     = 500;   // rps^2
}

bool DmakeMotecDrive::CanRecviveData() {
    if ( !is_open_can ) { return false; }
    CAN_DataFrame vco[ 200 ];
    ULONG         dwRel = CAN_ChannelReceive( dw_DeviceHandle, dw_Channel, vco, 200, 40 );
    if ( dwRel <= 0 ) { return false; }
    for ( ULONG i = 0; i < dwRel; i++ ) {
        QByteArray recv;
        for ( int k = 0; k < vco[ i ].nDataLen; k++ ) {
            recv.append( static_cast< char >( vco[ i ].arryData[ k ] ) );
        }
        emit sigShowCanData( recv, "Recv:" + QString::number( vco[ i ].uID, 16 ) );  //发送打印static_cast<short int>
        if ( vco[ i ].uID == 0X1A6 ) {
            // rpm
            short int recive_walk_speed = static_cast< short int >( ( vco[ i ].arryData[ 0 ] ) + ( vco[ i ].arryData[ 1 ] << 8 ) );
            // m/s
            fork_walk_reality_speed = static_cast< double >( recive_walk_speed ) / Walk_Wheel_Speed_Reduction_Radio / 60 * ( 2 * M_PI * Walk_Wheel_R );
        }

        // DMKE 舵轮脉冲反馈
        else if ( vco[ i ].uID == static_cast< unsigned int >( 0x580 + turn_drive_id ) ) {
            if ( ( vco[ i ].nDataLen == 8 ) && ( vco[ i ].arryData[ 0 ] == 0x43 ) && ( vco[ i ].arryData[ 1 ] == 0x64 ) && ( vco[ i ].arryData[ 2 ] == 0x60 ) && ( vco[ i ].arryData[ 3 ] == 0x00 ) ) {
                int recive_veer_puls = ( vco[ i ].arryData[ 4 ] ) + ( vco[ i ].arryData[ 5 ] << 8 ) + ( vco[ i ].arryData[ 6 ] << 16 ) + ( vco[ i ].arryData[ 7 ] << 24 );
                //（脉冲差值/一圈脉冲数）*2π /减速比
                fork_veer_reality_rad = -static_cast< double >( recive_veer_puls - Zero_Pulse ) / pulse_per * ( 2 * M_PI ) / Veer_Wheel_Speed_Reduction_Radio;
            }
        }
    }
    return true;
}

void DmakeMotecDrive::RegularGetData() {
    ULONG        i = 0;
    CanDataFrame frames[ 20 ];

    CanDataFrameSetForkRaiseSpeed( frames[ i++ ], fork_lft_speed );
    CanDataFrameSetKDSData( frames[ i++ ], kds_speed, kds_rise, kds_fall );

    /***********DMKE Angle Set*************/
    CanDataFrameGetCurPoseValue( frames[ i++ ], turn_drive_id );
    CanDataFrameSetPoModePosValue( frames[ i++ ], turn_drive_id, fork_calc_puls );
    CanDataFrameControlByte0F( frames[ i++ ], turn_drive_id );
    CanDataFrameControlByte3F( frames[ i++ ], turn_drive_id );

    CanSendData( frames, i );
}

void DmakeMotecDrive::CanSetSpeedAngle( double walk_speed, double veer_rad ) {

    fork_walk_setting_speed = walk_speed;
    fork_veer_setting_rad   = veer_rad;
    // 速度m/s = 给定速度（Walk_Wheel_Speed）* 60 / 周长（Walk_Wheel_C） *
    // 减速比（Walk_Wheel_Speed_Reduction_Radio）
    kds_speed = static_cast< short >( ( fork_walk_setting_speed * 60 / Walk_Wheel_C ) * Walk_Wheel_Speed_Reduction_Radio );
    // DMKE 脉冲值 = 弧度
    fork_calc_puls = static_cast< long >( -fork_veer_setting_rad * Veer_Wheel_Speed_Reduction_Radio * pulse_per / ( 2 * M_PI ) + Zero_Pulse );
}

void DmakeMotecDrive::CanDataFrameSetKDSData( CanDataFrame& frame, short& speed, uchar& kdsRise, uchar& kdsFall ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = static_cast< BYTE >( speed );
    sendbuffer[ 1 ]   = static_cast< BYTE >( speed >> 8 );
    sendbuffer[ 2 ]   = 0;
    sendbuffer[ 3 ]   = 0;
    sendbuffer[ 4 ]   = kdsRise;
    sendbuffer[ 5 ]   = kdsFall;
    sendbuffer[ 6 ]   = 0;
    sendbuffer[ 7 ]   = 0;
    frame.uID         = 0x226;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmakeMotecDrive::CanDataFrameSetForkRaiseSpeed( CanDataFrame& frame, uchar value ) {
    frame.uID           = 0x222;
    frame.nSendType     = 0;
    frame.bRemoteFlag   = 0;
    frame.bExternFlag   = 0;
    frame.nDataLen      = 1;
    frame.arryData[ 0 ] = value;
}

void DmakeMotecDrive::sltForkLiftDrive_Init() {
    if ( CanOpenAndStart() ) {  // open can drive
        emit sigSendTips( "DmakeMotecDrive 正确，CAN设备打开成功" );
    }
    else {
        emit sigSendTips( "DmakeMotecDrive 错误，CAN设备打开失败" );
        return;
    }
    ULONG        i = 0;
    CanDataFrame frames[ 20 ];
    CanDataFrameOperationalMode( frames[ i++ ], can_operation_enable );  //德马克电机操作模式
    /******DMKE==转向轮设置位置模式*******/
    long calc_speed = ( ( pos_mode_speed * 10 ) / 60 ) * pulse_per;
    long m_acce     = static_cast< long >( pos_mode_acc * pulse_per / 10 );
    long m_dece     = static_cast< long >( pos_mode_dec * pulse_per / 10 );
    CanDataFramePositionMode( frames[ i++ ], turn_drive_id );
    CanDataFrameSetPosModeSpeedValue( frames[ i++ ], turn_drive_id, calc_speed );
    CanDataFrameAcce( frames[ i++ ], turn_drive_id, m_acce );
    CanDataFrameDece( frames[ i++ ], turn_drive_id, m_dece );
    /******行走轮设定速度*******/

    CanDataFrameSetKDSData( frames[ i++ ], kds_speed, kds_rise, kds_fall );
    CanSendData( frames, i );

    emit sigSendTips( "提示，电机初始化完成！" );
    Utility::delayMs( 1000 );  // 非阻塞延时执行
    QTimer* timer50ms = new QTimer();
    connect( timer50ms, SIGNAL( timeout() ), this, SLOT( sltTimeOut() ) );
    timer50ms->start( time );
}

void DmakeMotecDrive::CanDataFramePositionMode( CanDataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = position_mode[ i ];
    }
}
void DmakeMotecDrive::CanDataFrameSetPosModeSpeedValue( CanDataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_position_mode_speed_value[ 0 ];
    sendbuffer[ 1 ]   = set_drive_position_mode_speed_value[ 1 ];
    sendbuffer[ 2 ]   = set_drive_position_mode_speed_value[ 2 ];
    sendbuffer[ 3 ]   = set_drive_position_mode_speed_value[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}
void DmakeMotecDrive::CanDataFrameAcce( CanDataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_accec[ 0 ];
    sendbuffer[ 1 ]   = set_drive_accec[ 1 ];
    sendbuffer[ 2 ]   = set_drive_accec[ 2 ];
    sendbuffer[ 3 ]   = set_drive_accec[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmakeMotecDrive::CanDataFrameDece( CanDataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_dece[ 0 ];
    sendbuffer[ 1 ]   = set_drive_dece[ 1 ];
    sendbuffer[ 2 ]   = set_drive_dece[ 2 ];
    sendbuffer[ 3 ]   = set_drive_dece[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmakeMotecDrive::CanDataFrameGetCurPoseValue( CanDataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_cur_pos[ i ];
    }
}

void DmakeMotecDrive::CanDataFrameSetPoModePosValue( CanDataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_position_mode_pos_value[ 0 ];
    sendbuffer[ 1 ]   = set_drive_position_mode_pos_value[ 1 ];
    sendbuffer[ 2 ]   = set_drive_position_mode_pos_value[ 2 ];
    sendbuffer[ 3 ]   = set_drive_position_mode_pos_value[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}
void DmakeMotecDrive::CanDataFrameControlByte0F( CanDataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_0F[ i ];
    }
}
void DmakeMotecDrive::CanDataFrameControlByte3F( CanDataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_3F[ i ];
    }
}
void DmakeMotecDrive::CanDataFrameOperationalMode( CanDataFrame& frame, BYTE value ) {
    frame.uID           = 0;
    frame.nSendType     = 0;
    frame.bRemoteFlag   = 0;
    frame.bExternFlag   = 0;
    frame.nDataLen      = 2;
    frame.arryData[ 0 ] = value;
    frame.arryData[ 1 ] = 0;
}
